function myDWA0(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str)
mapSize=data.mapSize;
mysize=50;
mycolor=[0,0,0];
disp('Dynamic Window Approach sample program start!!')

MapSize=size(map);

obstacleR=0.5*sqrt(2);% 冲突判定用的障碍物半径

tic;
% movcount=0;
% Main loop
figure
[p1,p2]=find(map==1);
for ii=1:length(p1)
    xi=p1(ii);
    yi=p2(ii);

    rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
end
% [p1,p2]=find(map==0);
% for ii=1:length(p1)
%     xi=p1(ii);
%     yi=p2(ii);
%     rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','w')
% end
hold on
no1=[];
no2=[];

plot(path(:,2),path(:,1),'-r','LineWidth',2)
if ~isempty(newObstacle)
    plot(newObstacle(:,2),newObstacle(:,1),'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
end
grid on;
set(gca,'YDir','reverse')	% 翻转Y轴
axis([-1,mapSize(1),-1,mapSize(2)])
axis equal
figure
% temp=0.2;
% temp1=path(1,:)-x(1:2)';
% temp2=obstacle(:,1)*0+0.05;
% temp3=newObstacle(:,1)*0+0.1;
%x(3)=cart2pol(temp1(1),temp1(2));
%sumD=0;
preT=5; %预测十个步长
[p1,p2]=find(map==1);
obstacle=[p1,p2];
path1=path;
nowP=path(1,:);
path(1,:)=[];
aimP=path(1,:);
v=aimP-nowP;
maxTheta=30; %单位时刻最大转向角度为30度
speed=1; %单位时刻速度
newPath=nowP;
path_M=[];
for i=1:5000
    % 更新动态障碍物位置
    if i<1000
        newObstacle=newObstacle+newObstacle_v;
        path_M=[path_M;newObstacle];
    end
    direction=getDirection(5);
    index=[];
    for j=1:length(direction(:,1))
        v0=direction(j,:);
        theta=acosd(dot(v0,v)/norm(v0)/norm(v));
        if abs(theta)>maxTheta
            index=[index;j];
        end
    end
    direction(index,:)=[];
    D_AimP=zeros(length(direction(:,1)),1)+inf;
    flag=zeros(length(direction(:,1)),1);
    maxT=zeros(length(direction(:,1)),1);
    path_preT=cell(length(direction(:,1)),1);
    for j=1:length(direction(:,1))
        newP=nowP;
        path_preT{j}=[path_preT{j};newP];
        for tt=1:preT
            dir=direction(j,:);
            newP=newP+dir/norm(dir)*speed;
            path_preT{j}=[path_preT{j};newP];
            D1=pdist2([obstacle;],newP);
            if min(D1)<obstacleR
                flag(j)=1;
                break;
            end
            if ~isempty(newObstacle)
                D2=pdist2([newObstacle],newP);
                if min(D2)<newObstacle_R
                    flag(j)=1;
                    break;
                end
            end
            if ~isempty(newObstacle_F)
                D3=pdist2([newObstacle_F],newP);
                if min(D3)<newObstacle_F_R
                    flag(j)=1;
                    break;
                end
            end
            D_AimP(j)=min(D_AimP(j),pdist2(aimP,newP));
        end
        maxT(j)=tt;
    end
    p=find(maxT>=5);
    %if ~isempty(p)
    % [~,noDir]=min(D_AimP(p));
    % dir=direction(p(noDir),:);
    % v=dir;
    % else
    dir=aimP-nowP;
    dir=dir/norm(dir)*speed;
    % end
    nowP=nowP+dir/norm(dir)*speed;
    newPath=[newPath;nowP];
    % 是否到达目的地
    while norm(nowP-aimP)<1
        if isempty(path)
            %disp('Arrive Goal!!');
            break;
        else
            aimP=path(1,:);
            path(1,:)=[];
        end

    end
    if norm(nowP-aimP)<1
        if isempty(path)
            disp('Arrive Goal!!');
            break;
        end
    end
    %====Animation====
    hold off;
    plot(0,0)
    hold on;
    ArrowLength=0.5;%
    % 机器人
    [p1,p2]=find(map==1);
    for ii=1:length(p1)
        xi=p1(ii);
        yi=p2(ii);
        rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
    end
    % [p1,p2]=find(map==0);
    % for ii=1:length(p1)
    %     xi=p1(ii);
    %     yi=p2(ii);
    %     rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','w')
    % end
    if ~isempty(newObstacle)
        xi=newObstacle(1);
        yi=newObstacle(2);
        rectangle('Position',[yi-newObstacle_R/2,xi-newObstacle_R/2,newObstacle_R,newObstacle_R],'FaceColor','y')
    end
    if ~isempty(newObstacle_F)
        xi=newObstacle_F(1);
        yi=newObstacle_F(2);
        rectangle('Position',[yi-newObstacle_F_R/2,xi-newObstacle_F_R/2,newObstacle_F_R,newObstacle_F_R],'FaceColor','c')
    end
    %     quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');
    hold on;
    % for j=1:length(direction(:,1))
    %     plot(path_preT{j}(:,2),path_preT{j}(:,1),'-g','LineWidth',2);
    % end

    plot(aimP(2),aimP(1),'*r');
    plot(path1(:,2),path1(:,1),'--m','LineWidth',2)
    plot(newPath(:,2),newPath(:,1),'-b','LineWidth',2)
    if ~isempty(path_M)
        plot(path_M(:,2),path_M(:,1),'-y','LineWidth',2)
    end
    %plot(newObstacle(:,2),newObstacle(:,1),'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
    grid on;
    set(gca,'YDir','reverse')	% 翻转Y轴

    axis equal
    axis([-1,mapSize(2),-1,mapSize(1)])
    title([str,',t=',num2str(i)])
    drawnow;
end
hold off;
plot(0,0)
hold on;
ArrowLength=0.5;%
% 机器人
[p1,p2]=find(map==1);
for ii=1:length(p1)
    xi=p1(ii);
    yi=p2(ii);
    rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
end
% [p1,p2]=find(map==0);
% for ii=1:length(p1)
%     xi=p1(ii);
%     yi=p2(ii);
%     rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','w')
% end
if ~isempty(newObstacle)
    xi=newObstacle(1);
    yi=newObstacle(2);
    rectangle('Position',[yi-newObstacle_R/2,xi-newObstacle_R/2,newObstacle_R,newObstacle_R],'FaceColor','r')
end
if ~isempty(newObstacle_F)
    xi=newObstacle_F(1);
    yi=newObstacle_F(2);
    rectangle('Position',[yi-newObstacle_F_R/2,xi-newObstacle_F_R/2,newObstacle_F_R,newObstacle_F_R],'FaceColor','c')
end
%     quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');
hold on;
% for j=1:length(direction(:,1))
%     plot(path_preT{j}(:,2),path_preT{j}(:,1),'-g','LineWidth',2);
% end
newPath=[newPath;aimP];
plot(aimP(2),aimP(1),'*r');
plot(path1(:,2),path1(:,1),'--m','LineWidth',2)
plot(newPath(:,2),newPath(:,1),'-b','LineWidth',2)
if ~isempty(path_M)
    plot(path_M(:,2),path_M(:,1),'-y','LineWidth',2)
end
%plot(newObstacle(:,2),newObstacle(:,1),'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
grid on;
set(gca,'YDir','reverse')	% 翻转Y轴

axis equal
axis([-1,mapSize(2),-1,mapSize(1)])
title([str,',t=',num2str(i)])
drawnow;
tt=toc;
sumD=0;
for i=1:length(newPath(:,1))-1
    sumD=sumD+norm(newPath(i,:)-newPath(i+1,:));
end

disp([str,'运行时间：',num2str(tt)])
disp([str,'总距离：',num2str(sumD)])
end